#include <iostream>

#include "predictor_const_v.h"
#include "predictor_link.h"

using namespace std;

Predictor_const_v::Predictor_const_v( std::string id ) : Predictor_base( id )
{
  _string_type = "Constant velocity";
  add_input_link( new Predictor_link( PARAMETER ) ); // velocity
  add_input_link( new Predictor_link( INPUT ) ); // position
  _predicted_inputs.resize( _input_links.size() );

  _complexity = 2.0;
}

Predictor_const_v::~Predictor_const_v()
{
}

void Predictor_const_v::save_state( float time )
{
  save_input( time );
}

void Predictor_const_v::predict( float time )
{
  cout << "Predictor: " << _string_id << endl;
  _predicted_inputs[0] = _input_links[0]->get_value(); // input velocity
  cout << "Predicted velocity: " << _predicted_inputs[0] << endl;
  // predict new position
  _predicted_inputs[1] = _saved_inputs[1] + _input_links[0]->get_value()*(time-_saved_time);
  cout << "Predicted position: " << _predicted_inputs[1] << endl;
}

void Predictor_const_v::activate_output_link( void )
{
  if(_active) {
    _output_link->activate();
  } else {
    //cout << "  " << _string_id << ":  De-activating output " << endl;
    _output_link->deactivate();
  }
}
